#include "RTUMaster.hpp"
#include "crc16.hpp"
#include <cstdint>
#include <cstring>

using namespace Modbus;

RTUMaster::RTUMaster(char *buffer,unsigned int buffer_size,PortableStrategy *portable_strategy){
    this->portable_strategy = portable_strategy;
    this->rx_buffer_information.buffer = buffer,
    this->rx_buffer_information.buffer_size = buffer_size;
    this->rx_buffer_information.rx_size = 0;
    this->rx_buffer_information.payload_offset = 0;
    this->rx_buffer_information.payload_size = 0;
}

unsigned int RTUMaster::sendRequest(int slave_address,const char *buffer,unsigned int length,int timeout){
    if ( this->portable_strategy == nullptr ){
        this->error_number = static_cast<int>(ErrorNumber::NullPortable);
        return 0;
    }
    uint8_t u8_slave_address = static_cast<uint8_t>(slave_address);
    uint16_t crc16 = 0x0000;
    CRC16Calculator crc16_calculator;
    (void)crc16_calculator.start();
    (void)crc16_calculator.feed(u8_slave_address);
    (void)crc16_calculator.feed(buffer,length);
    crc16 = crc16_calculator.get();
    (void)timeout;
    /* Send slave address. */
    if ( this->portable_strategy->send(u8_slave_address) != true ){
        this->error_number = static_cast<int>(ErrorNumber::IOError);
        return 0;
    }
    /* Send payload. */
    for ( int n_byte = 0; n_byte < length; n_byte++ ){
        if ( this->portable_strategy->send(buffer[n_byte]) != true ){
            this->error_number = static_cast<int>(ErrorNumber::IOError);
            return 0;
        }
    }
    /* Send crc-16. */
    if ( this->portable_strategy->send(static_cast<unsigned char>(crc16 & 0xff)) != true ){
        this->error_number = static_cast<int>(ErrorNumber::IOError);
        return 0;
    }
    if ( this->portable_strategy->send(static_cast<unsigned char>((crc16 >> 8) & 0xff)) != true ){
        this->error_number = static_cast<int>(ErrorNumber::IOError);
        return 0;
    }
    /* Cut frame. */
    if ( this->portable_strategy->cut() != true ){
        this->error_number = static_cast<int>(ErrorNumber::IOError);
        return 0;
    }
    this->error_number = static_cast<int>(ErrorNumber::None);
    return length;
}

unsigned int RTUMaster::receiveResponse(int slave_address,char *buffer,unsigned int length,int timeout){
    if ( this->portable_strategy == nullptr ){
        this->error_number = static_cast<int>(ErrorNumber::NullPortable);
        return 0;
    }
    if ( this->rx_buffer_information.buffer == nullptr || this->rx_buffer_information.buffer_size <= 0 ){
        this->error_number = static_cast<int>(ErrorNumber::BufferError);
        return 0;
    }
    uint8_t u8_slave_address = static_cast<uint8_t>(slave_address);
    unsigned int size_copied = 0;
    if ( timeout < 0 ){
        while ( true ){
            if ( this->rx_buffer_information.payload_size > 0 ){
                if ( this->rx_buffer_information.buffer[0] == u8_slave_address ){
                    size_copied = (this->rx_buffer_information.payload_size > length) ? length : this->rx_buffer_information.payload_size;
                    memcpy(buffer,this->rx_buffer_information.buffer + this->rx_buffer_information.payload_offset,size_copied);
                    return size_copied;
                }
            }
        }
    }else{
        int n_retry = 0;
        do {
            if ( this->rx_buffer_information.payload_offset > 0 ){
                if ( this->rx_buffer_information.buffer[0] == u8_slave_address ){
                    size_copied = (this->rx_buffer_information.payload_size > length) ? length : this->rx_buffer_information.payload_size;
                    memcpy(buffer,this->rx_buffer_information.buffer + this->rx_buffer_information.payload_offset,size_copied);
                    this->error_number = static_cast<int>(ErrorNumber::None);
                    return size_copied;
                }
            }
            n_retry += 1;
            if ( timeout == 0 ){
                break;
            }
        } while ( n_retry <= timeout );
        this->error_number = static_cast<int>(ErrorNumber::Timeout);
    }
    return 0;
}

bool RTUMaster::feed(char chr){
    if ( this->rx_buffer_information.rx_size >= this->rx_buffer_information.buffer_size ){
        /* Buffer is full. */
        this->error_number = static_cast<int>(ErrorNumber::BufferError);
        return false;
    }
    this->rx_buffer_information.buffer[this->rx_buffer_information.rx_size++] = chr;
    this->error_number = static_cast<int>(ErrorNumber::None);
    return true;
}

bool RTUMaster::cut(void){
    if ( this->rx_buffer_information.rx_size <= 2 ){
        this->rx_buffer_information.rx_size = 0;
    }
    /* Check crc-16. */
    CRC16Calculator crc16_calculator;
    crc16_calculator.start();
    uint16_t crc16_rx = this->rx_buffer_information.buffer[this->rx_buffer_information.rx_size - 2] | (this->rx_buffer_information.buffer[this->rx_buffer_information.rx_size - 1] << 8);
    crc16_calculator.feed(this->rx_buffer_information.buffer,this->rx_buffer_information.rx_size - 2);
    uint16_t crc16_cal = crc16_calculator.get();
    if ( crc16_cal == crc16_rx ){
        this->rx_buffer_information.payload_offset = 0;
        this->rx_buffer_information.payload_size = this->rx_buffer_information.rx_size - 2;
    }
    this->rx_buffer_information.rx_size = 0;
}

int RTUMaster::getErrorNumber(void){
    return this->error_number;
}
